#include "PositionSensor.hpp"
#include "MiscUtils/TdrMisc/SEngine.h"

using namespace std;

PositionSensor::PositionSensor(GeneralSlave* ref) {
	genSlaveInit(ref);
	setDefaultFriendlyName();
}

PositionSensor::~PositionSensor() {

}

PositionSensor::PositionReading::~PositionReading() {
	//if (sensorData != nullptr) {

	//}
}

int PositionSensor::PositionReading::set(float val) {
	positionMM = val;
	readOutTimeMs = TStopWatch::getTimeSinceStart();
	reliable = 1;
	return 1;
}


int PositionSensor::PositionReading::parse(std::string reply) {
	return 1;
}

int PositionSensor::readOut(uint64_t loopIndex) {
	return readOutInIdle(loopIndex);
}

int PositionSensor::readOutDebugDetail() {
	string cmd = "ecdr_abs read 0 -d";
	if (sendString(cmd) > 0) {
		string rpl;
		if (receive(&rpl) > 0) {
			if (_reading.parse(rpl) > 0) {
				_generalStat.readingIndex = 0;
			}
			return 1;
		}
	}

	return -1;
}

int PositionSensor::readOutInIdle(uint64_t loopIndex) {
	if ((loopIndex > 0) && (loopIndex <= _generalStat.readingIndex)) {
		return 0;
	}
	
	if (int state = preSendingConfig() <= 0) {
		return state;
	}
	string cmd = "position ?\r\n";
	if (sendString(cmd) > 0) {
		if (_meta.isSimulated > 0) {
			float reading = _measuredRobot.simuCallback(getReferenceName(), _measuredRobot.slaveRefName, _measuredRobot.axisIndex);
			string readingStr = "reading=" + TFormater::float2str(reading, -1, 3) + "\r\n";
			_callbacks.serialReceiveCB(true, readingStr.data(), readingStr.size(), nullptr, 0);
			_reading.set(reading);
			if (loopIndex > 0) {
				_generalStat.readingIndex = loopIndex;
			}
			return 1;
		}
		string rpl;
		if (receive(&rpl) > 0) {
			if (_reading.parse(rpl) > 0) {
				if (loopIndex > 0) {
					_generalStat.readingIndex = loopIndex;
				}
			}
			return 1;
		}
	}

	return -1;
}